package lego.jbv;

import java.util.ArrayList;
import lejos.nxt.Button;
import lejos.nxt.ButtonListener;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.Navigator;
import lejos.robotics.navigation.Waypoint;
import lejos.util.Delay;

public class calibationMain {
	private static final double wheelDiameter = 5.51;
	private static final double trackWidth = 16.01;
	private static DifferentialPilot pilot;
	private static Navigator navigator;
	
	public static void main(String[] args) {
		init();
		pilot = new DifferentialPilot(wheelDiameter, trackWidth, Motor.B, Motor.C);
		navigator = new Navigator(pilot);
		
		navigator.addWaypoint(new Waypoint(0, 0));
		navigator.addWaypoint(new Waypoint(40, 0));
		navigator.addWaypoint(new Waypoint(20, 20));
		navigator.addWaypoint(new Waypoint(20, -10));
		navigator.addWaypoint(0, 0, 0);
		navigator.addWaypoint(new Waypoint(40, 0));
		navigator.addWaypoint(new Waypoint(20, 20));
		navigator.addWaypoint(new Waypoint(20, -10));
		navigator.addWaypoint(0, 0, 0);
		
		//navigator.addWaypoint(0, 0, 1440);
		
		LCD.drawString("Press any button", 0, 0);
		Button.waitForAnyPress();
		LCD.clear();
		LCD.drawString("Driving", 0, 0);
		
		Delay.msDelay(200);
		navigator.followPath();
		while(true)
		{}
	}
	
	private static void init()
    {
    	Button.ESCAPE.addButtonListener(new ButtonListener() {
			
			@Override
			public void buttonReleased(Button b) {
				LCD.clear();
			    System.exit(0);				
			}
			
			@Override
			public void buttonPressed(Button b) {
				// TODO Auto-generated method stub
				
			}
		});
    }
}
